package se.lnu.axez.physics.bullet;

import java.util.ArrayList;
import java.util.HashMap;
import java.util.List;
import java.util.Map;
import java.util.concurrent.Executors;
import java.util.concurrent.ScheduledExecutorService;
import java.util.concurrent.ScheduledFuture;
import java.util.concurrent.TimeUnit;

import se.lnu.axez.Constants;
import se.lnu.axez.FloatTriple;
import se.lnu.axez.IControl;

public final class DiscreteDynamicsWorld {
//	public static final String LOG_TAG = "DiscreteDynamicsWorld";
	public static final int COLLISION_FILTER_DEFAULT = 1;
	public static final int COLLISION_FILTER_STATIC = 2;
	public static final int COLLISION_FILTER_ALL = -1;
	private static final float[] tmpMatrix = new float[Constants.GL_MATRIX_SIZE * 2];
	private static final int MAX_SUB_STEPS = 10;
	private static final float TIME_STEP = 1f/25f;
	private static final long SIMULATION_DELAY = 40L;
	
	private final List<RigidBody> rigidBodies = new ArrayList<RigidBody>();
	private final Map<Integer, RigidBody> idToRigidBody = new HashMap<Integer, RigidBody>();
	private final ScheduledExecutorService mSimulationScheduler = Executors.newScheduledThreadPool(1);

	private final ISimulationCallback mSimulationCallback = new ISimulationCallback() {
		@Override
		public void onRigidBodyUpdate(int rigidBodyID, float[] orientationMatrix) {
			if (!idToRigidBody.containsKey(rigidBodyID)) {
				return;
			}
			
			RigidBody rigidBody = idToRigidBody.get(rigidBodyID);
			rigidBody.update(orientationMatrix);
		}
	};
	
	private final Runnable mSimulationRunnable = new Runnable() {
		@Override
		public void run() {
			doSimulation(TIME_STEP);
			if (mScheduledSimulation.isCancelled()) {
				return;
			}
			
			mScheduledSimulation = mSimulationScheduler.schedule(mSimulationRunnable, SIMULATION_DELAY, TimeUnit.MILLISECONDS);
		}
	};
	
	private ScheduledFuture<?> mScheduledSimulation;
	private FloatTriple gravity;
	private int worldID;
	
	DiscreteDynamicsWorld(FloatTriple gravity) {
		this.gravity = gravity;
	}

	public synchronized void startSimulation() {
		mScheduledSimulation = mSimulationScheduler.schedule(mSimulationRunnable, SIMULATION_DELAY, TimeUnit.MILLISECONDS);
	}
	
	public synchronized void stopSimulation() {
		if (mScheduledSimulation != null) {
			mScheduledSimulation.cancel(false);
			mScheduledSimulation = null;
		}
	}
	
	public synchronized RigidBody createRigidBody(
			IControl control, 
			CollisionShape collisionShape,
			float mass) {
		RigidBody rigidBody = new RigidBody(this, control, collisionShape, mass);
		this.rigidBodies.add(rigidBody);
		return rigidBody;
	}

	int getWorldID() {
		return worldID;
	}

	synchronized void registerRigidBody(int rigidBodyId, RigidBody rigidBody) {
		this.idToRigidBody.put(rigidBodyId, rigidBody);
	}
	
	synchronized void doSimulation(float timeStep) {
		BulletFacade bulletFacade = BulletFacade.getInstance();
		bulletFacade.doSimulation(mSimulationCallback, this.worldID, timeStep, MAX_SUB_STEPS);
	}
	
	synchronized void create(BulletFacade bulletFacade) {
		this.worldID = bulletFacade.createDiscreteDynamicsWorld(gravity);

		for (RigidBody rigidBody : this.rigidBodies) {
			rigidBody.create(bulletFacade, tmpMatrix);
		}
	}
	
	synchronized void destroy(BulletFacade bulletFacade) {
		for (RigidBody rigidBody : this.rigidBodies) {
				rigidBody.destroy(bulletFacade);
		}
		
		this.rigidBodies.clear();
		this.idToRigidBody.clear();
		
		bulletFacade.destroyDiscreteDynamicsWorld(this.worldID);
		this.worldID = 0;
	}
}
